#include "Wire.h"
#include "def.h"
#include "sensor.h"
//#include "pid.h"

#define USEPID
#ifdef USEPID
#include "PID_v1.h"
double Setpoint, Input, Output;
double aggKp=4, aggKi=0.2, aggKd=1;
double kP=1, kI=0.05, kD=0.25;
PID myPID(&Input, &Output, &Setpoint, kP, kI, kD, DIRECT);
#endif

double roll, pitch, yaw;
int mod_output = 0;
#include "serialcmd.h"

#define USESERVO
#ifdef USESERVO
#include <Servo.h> 
Servo myservo;
#endif

void setup() {
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  SensorInit();

  Setpoint = 90;
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(0, 180);

#ifdef USESERVO
  myservo.attach(9);
  myservo.write(90);
  delay(1000);
#endif
}

void loop() {
  ReadSerial();
  SplitCmd(buf);

  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  if(SensorRead()) {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    roll = Roll(q.x, q.y, q.z, q.w);
    //pitch = Pitch(q.x, q.y, q.z, q.w);
    //yaw = Yaw(q.x, q.y, q.z, q.w);

    Serial.print("PIDSmodo:\t");
    Serial.print(kP, 4);
    Serial.print("|");
    Serial.print(kI, 4);
    Serial.print("|");
    Serial.print(kD, 4);
    Serial.print("|");
    Serial.print(Setpoint);
    Serial.print("|");
    Serial.print(mod_output);

    Serial.print("\tROLL:");
    Serial.print(roll);

    //Output = updatePid(roll);
    Input = 90 - roll;


    double gap = abs(Setpoint - Input); //distance away from setpoint
    if(gap < 10)
    {
      myPID.SetTunings(kP, kI, kD);
    }
    else
    {
      myPID.SetTunings(aggKp, aggKi, aggKd);
    }

    myPID.Compute();

    Serial.print("\tOUTOPUT:");
    Serial.print(Output);


#ifdef USESERVO
    //int val = map(mod_output + Output, 0, 255, 0, 180);
    int val = mod_output + Output;
    Serial.print("\tSERVO:");
    Serial.print(val);
    myservo.write(val);
#endif

    Serial.println();
  }
}










